function [ntr_param]= add_control_noise(tr_param,Q, addnoise)
% Add random noise to nominal control values

if addnoise == 1
    for i=1:3
%        ntr_param(i) = tr_param(i) + (randn(1)*(sqrt(Q(i,i)))); % if assume Q is diagonal
        ntr_param(i) = tr_param(i) + (rand(1)*sign(randn(1))*(sqrt(Q(i,i)))); % if assume Q is diagonal
    end
    for i=4:6
        if abs(tr_param(i)) > 0
    %        ntr_param(i) = tr_param(i) + (randn(1)*(sqrt(Q(i,i)))); % if assume Q is diagonal
            ntr_param(i) = tr_param(i) + ((rand(1)*sign(randn(1)))*(sqrt(Q(i,i))*180/pi))*pi/180; % if assume Q is diagonal        
        elseif abs(tr_param(i)) == 0
            ntr_param(i) = tr_param(i);
        end
        
    end
end
